#include <iostream>
using namespace std;
#include "slamBase.hpp"

#include <opencv2/core/eigen.hpp>

#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

// Eigen !
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char **argv)
{
    ParameterReader pr;
    FRAME frame1, frame2;
    frame1.rgb = cv::imread("../data/rgb1.png");
    frame2.rgb = cv::imread("../data/rgb2.png");
    frame1.depth = cv::imread("../data/depth1.png", cv::IMREAD_UNCHANGED);
    frame2.depth = cv::imread("../data/depth2.png", cv::IMREAD_UNCHANGED);
    // 提取特征计算描述子
    cout << "extracting features" << endl;
    string detector = pr.config["detector"].as<string>();
    string descriptor = pr.config["descriptor"].as<string>();
    computeKeyPointsAndDesp(frame1, detector, descriptor);
    computeKeyPointsAndDesp(frame2, detector, descriptor);
    // 相机内参
    CAMERA_INTRINSIC_PARAMETERS camera;
    camera.cx = pr.config["camera"]["cx"].as<double>();
    camera.cy = pr.config["camera"]["cy"].as<double>();
    camera.fx = pr.config["camera"]["fx"].as<double>();
    camera.fy = pr.config["camera"]["fy"].as<double>();
    camera.scale = pr.config["camera"]["scale"].as<double>();
    // 求解PnP
    cout << "solving pnp" << endl;
    RESULT_OF_PNP result = estimateMotion(frame1, frame2, camera, pr);

    cout << result.rvec << endl << result.tvec << endl;
    // 处理result
    // 将旋转向量转化为旋转矩阵
    cv::Mat R;
    cv::Rodrigues(result.rvec, R);
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);

    // 将平移向量和旋转矩阵转换成变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    Eigen::AngleAxisd angle(r);
    cout << "translation" << endl;
    Eigen::Translation<double, 3> trans(result.tvec.at<double>(0, 0), result.tvec.at<double>(1, 0),
                                        result.tvec.at<double>(2, 0));
    T = angle;
    T(0, 3) = result.tvec.at<double>(0, 0);
    T(1, 3) = result.tvec.at<double>(1, 0);
    T(2, 3) = result.tvec.at<double>(2, 0);

    // 转换点云
    cout << "converting image to clouds" << endl;
    PointCloud::Ptr cloud1 = image2PointCloud(frame1.rgb, frame1.depth, camera);
    PointCloud::Ptr cloud2 = image2PointCloud(frame2.rgb, frame2.depth, camera);

    // 合并点云
    cout << "combining clouds" << endl;
    PointCloud::Ptr output(new PointCloud());
    pcl::transformPointCloud(*cloud1, *output, T.matrix());
    *output += *cloud2;
    pcl::io::savePCDFile("../data/results/result.pcd", *output);
    cout << "Final result saved." << endl;

    pcl::visualization::CloudViewer viewer("viewer");
    viewer.showCloud(output);
    while (!viewer.wasStopped())
    {
    }
    return 0;
}